#include "RigidBody.h"
#include <assert.h>

namespace poly
{
	RigidBody::RigidBody()
	{
		pos = float3(0,0,0);
		vel = float3(0,0,0);
		acc = float3(0,0,0);
		orientation = identity33();
		rotVel = float3(0,0,0);
		rotMomentum = float3(0,0,0);
		invMass = 1;
		invLocalInertia = identity33();
		force = float3(0,0,0);
		torque = float3(0,0,0);
		elasticity = 1.f;
		friction = 0.f;
	}

	float33 RigidBody::invInertia() const
	{
		float33 invOrientation = transpose( orientation );
		float33 out = orientation * invLocalInertia * invOrientation;
		return out;
	}

	void RigidBody::addForce(float3 const &f)
	{
		force += f;
	}

	void RigidBody::addForce(float3 const &f, float3 const &p)
	{
		force += f;
		torque += cross( p-pos, f );
	}

	void RigidBody::addTorque(const float3 &t)
	{
		torque += t;
	}

	void RigidBody::addLinearImpulse(float3 const &f)
	{
		vel += f*invMass;
	}

	void RigidBody::addLinearImpulse(float3 const &f, float3 const &p)
	{
		vel += f*invMass;
		addRotationalImpulse(cross(p-pos,f));
	}

	void RigidBody::addRotationalImpulse(float3 const &t)
	{
		rotMomentum += t;
		rotVel = rotMomentum * invInertia();
	}

	void RigidBody::addForceLocal( float3 const & f )
	{
		float3 fw =  f * orientation;
		addForce(fw);

	}

	void RigidBody::addForceLocal( float3 const & f, float3 const & p )
	{
		float3 fw = f * orientation;
		float3 pw = transformCoord( p, world() );
		addForce(fw, pw);
	}

	void RigidBody::addTorqueLocal( float3 const & t )
	{
		float3 tw = t * orientation;
		addTorque(tw);
	}

	void RigidBody::addLinearImpulseLocal( float3 const & f )
	{
		float3 fw = f * orientation;
		addLinearImpulse(fw);
	}

	void RigidBody::addLinearImpulseLocal( float3 const & f, float3 const &p )
	{
		float3 fw = f * orientation;
		float3 pw = transformCoord( p, world() );
		addLinearImpulse(fw,pw);
	}

	void RigidBody::addRotationalImpulseLocal( float3 const &t )
	{
		float3 tw = t * orientation;
		addRotationalImpulse(tw);
	}

	void RigidBody::displace( float3 const & d )
	{
		pos += d;
	}

	void RigidBody::rotate( float3 const & r )
	{
		orientation = rotationAxis( r, length(r) ) * orientation;
	}

	void RigidBody::resolveCollision( RigidBody &rigidBody,
		Contact const & contact )
	{
		float c1 = -invMass / (invMass + rigidBody.invMass );
		float c2 = +rigidBody.invMass / (invMass + rigidBody.invMass );

		assert( invMass != 0 || c1 == 0 );
		assert( rigidBody.invMass != 0 || c2 == 0 );

		displace
			( -contact.seperation * invMass / (invMass + rigidBody.invMass) );

		rigidBody.displace
			( +contact.seperation * rigidBody.invMass / (invMass + rigidBody.invMass ) );

		float elast = elasticity * rigidBody.elasticity;
		float frict = friction * rigidBody.friction;

		float3 ra = contact.point - pos;
		float3 rb = contact.point - rigidBody.pos;

		float3 va = cross( rotVel, ra ) + vel;
		float3 vb = cross( rigidBody.rotVel, rb ) + rigidBody.vel;
		float3 n = contact.axis;

		float vrel = dot( n, va - vb );
		if( vrel < 0 ) return;

		float numerator = -(1 + elast) * vrel;

		float term1 = invMass;
		float term2 = rigidBody.invMass;
		float term3 = dot( n, cross( cross( ra, n) * invInertia(), ra ) );
		float term4 = dot( n, cross( cross( rb, n) * rigidBody.invInertia(), rb ) );

		float j = numerator / ((term1 + term2)*lengthSq(n) + term3 + term4);
		float3 imp = j * n;

		addLinearImpulse( +imp, contact.point );
		rigidBody.addLinearImpulse( -imp, contact.point );

		float3 vab = vb - va;
		float3 tang_vel = vab - dot(vab, n)*n;
		float3 tang_vel_dir = normalize(tang_vel);
		float3 fric_imp = tang_vel * frict;

		addForce( +fric_imp, contact.point );
		rigidBody.addForce( -fric_imp, contact.point );
	}

	void RigidBody::resolveJoint(RigidBody &RB, float3 const &jntPs )
	{
		float3 r1 = jntPs - pos;
		float3 r2 = jntPs - RB.pos;

		// j * A = vb - va -> j = (vb-va)*inverse(A)
		float33 M = identity33()*(invMass + RB.invMass);

		float33 crossing1 = crossing( r1 );
		float33 crossing2 = crossing( r2 );

		float33 I1 = -crossing1 * invInertia() * crossing1;
		float33 I2 = -crossing2 * RB.invInertia() * crossing2;

		float3 v1 = vel + cross( rotVel, r1 );
		float3 v2 = RB.vel + cross( RB.rotVel, r2 );

		float33 factor = inverse( M + I1 + I2 );
		float3 imp = (v2-v1) * factor;

		addLinearImpulse( +imp, jntPs );
		RB.addLinearImpulse( -imp, jntPs );
	}

	void RigidBody::resolveSlide( RigidBody &RB, float3 const & slidePt, float3 const & slideDir )
	{
		float3 normal = normalize( cross( slideDir, float3(0, 0, 1) ) );

		float3 r1 = slidePt - pos;
		float3 r2 = slidePt - RB.pos;

		float3 v1 = vel + cross( rotVel, r1 );
		float3 v2 = RB.vel + cross( RB.rotVel, r2 );

		float term1 = invMass + RB.invMass;
		float term2 = - dot( cross( cross( normal, r1 ) * invInertia(), r1 ), normal );
		float term3 = - dot( cross( cross( normal, r2 ) * RB.invInertia(), r2 ), normal );

		float imp = dot(v2-v1, normal) / (term1 + term2 + term3);

		addLinearImpulse( +imp * normal, slidePt );
		RB.addLinearImpulse( -imp * normal, slidePt );
	}

	void RigidBody::step(float dt)
	{
		pos += vel * dt;
		addLinearImpulse( force * dt );

		orientation = smallRotation( rotVel * dt ) * orientation;

		addRotationalImpulse( torque * dt );

		resetForces();
	}

	void RigidBody::resetForces()
	{
		force = float3(0,0,0);
		torque = float3(0,0,0);
	}

	float44 RigidBody::world() const
	{
		float44 out;
		out = augment(orientation) * translation( pos );
		return out;
	}
} // end of poly
